﻿using System;
using System.Collections.Generic;
using System.Text;
using PotentialFieldDriver.robotrace.environment.interfaces;
using System.Collections;
using System.Collections.ObjectModel;
using RoBOSSCommunicator;
using System.Globalization;
using Geometry;
using PotentialFieldDriver.robotrace.utils.geometry;
using System.Drawing;
using PotentialFieldDriver.robotrace.utils.pitch;

namespace PotentialFieldDriver.robotrace.environment.implementation
{
    class Pitch : PitchInterface
    {
        private List<Wall2D> walls;
        private Cell[][] bitmap;
        private RectangleF pitchArea;
        private int noOfcolumns;
        private int noOfRows;
        private bool shouldRecomputeBitmap = false;
        private double cellWidth;
        private double cellHeight;
        private List<Point2D> robotsStartingPositions = new List<Point2D>();

        public Pitch()
        {
            walls = new List<Wall2D>();
            pitchArea = findTheMostSuitableArea();
        }

        public Pitch(Communicator communicator)
            : this()
        {
            Geom[] geoms = communicator.environmentGeoms;
            if (geoms != null)
            {
                foreach (Geom geom in geoms)
                {

                    Wall2D wall = XmlValuesMapper.mapGeomFromXmlToWall2D(geom);
                    if (wall == null)
                    {
                        Console.WriteLine("WARN: not a wall");
                    }
                    else
                    {
                        walls.Add(wall);
                    }

                    //Console.WriteLine("Wall found: (x1, y1) - (x2, y2) = (" + xPosition + ", " + yPosition + ") - (" + (xPosition +xSize) + ", " + (yPosition + ySize) + ")" 
                }
            }
            pitchArea = findTheMostSuitableArea();
            foreach (Robot robot in communicator.robots)
            {
                robotsStartingPositions.Add(XmlValuesMapper.mapRobotStartingPositionFromXmlToPoint2D(robot));
            }
        }

        private RectangleF findTheMostSuitableArea()
        {
            if (walls == null || walls.Count == 0)
            {
                return new RectangleF(0.0f, 0.0f, 2.2f, 1.8f);
            }
            double minX = double.MaxValue;
            double minY = double.MaxValue;
            double maxX = double.MinValue;
            double maxY = double.MinValue;

            foreach (Wall2D wall in walls)
            {
                minX = wall.getMinX() < minX ? wall.getMinX() : minX;
                maxX = wall.getMaxX() > maxX ? wall.getMaxX() : maxX;
                minY = wall.getMinY() < minY ? wall.getMinY() : minY;
                maxY = wall.getMaxY() > maxY ? wall.getMaxY() : maxY;
            }

            return new RectangleF((float)(minX), (float)(minY), (float)(maxX - minX), (float)(maxY - minY));
        }

        public ReadOnlyCollection<Wall2D> getWalls()
        {
            return walls.AsReadOnly();
        }

        public void addWall(Wall2D newWall)
        {
            lock (walls)
            {
                walls.Add(newWall);
                pitchArea = findTheMostSuitableArea();
            }
        }

        public void clearWalls()
        {
            lock (walls)
            {
                walls.Clear();
                pitchArea = findTheMostSuitableArea();
            }
        }



        public Cell[][] getObstaclesBitmap(int numberOfColumns, int numberOfRows)
        {
            if (this.noOfcolumns == numberOfColumns && this.noOfRows == numberOfRows && bitmap != null && !shouldRecomputeBitmap)
            {
                return bitmap;
            }
            return generateBitmap(numberOfRows, numberOfColumns);
        }

        private Cell[][] generateBitmap(int numberOfRows, int numberOfColumns)
        {
            this.noOfRows = numberOfRows;
            this.noOfcolumns = numberOfColumns;
            bitmap = new Cell[noOfcolumns][];
            for (int i = 0; i < noOfcolumns; i++)
            {
                bitmap[i] = new Cell[noOfRows];
            }
            cellWidth = pitchArea.Width / numberOfColumns;
            cellHeight = pitchArea.Height / numberOfRows;

            for (int i = 0; i < noOfcolumns; i++)
            {
                for (int j = 0; j < noOfRows; j++)
                {
                    double upperLeftX = pitchArea.X + i * cellWidth;
                    double upperLeftY = pitchArea.Y + j * cellHeight;
                    Cell cell = new Cell(new Point2D(upperLeftX, upperLeftY), cellWidth, cellHeight, getWalls());
                    cell.Reachable = false;
                    bitmap[i][j] = cell;
                }
            }
            if (robotsStartingPositions.Count > 0)
            {
                excludeNonReachableFields(bitmap, robotsStartingPositions);
            }
            return bitmap;
        }

        private void excludeNonReachableFields(Cell[][] bitmap, List<Point2D> robotsStartingPositions)
        {
            foreach (Point2D robotPosition in robotsStartingPositions)
            {
                int column = mapRobotXPositionToCellColumnNo(robotPosition.x);
                int row = mapRobotYPositionToCellRowNo(robotPosition.y);
                dfs(bitmap, column, row);
            }
            /*for (int i = 0; i < bitmap.Length; i++)
            {
                for (int j = 0; j < bitmap[i].Length; j++)
                {
                    Cell cell = bitmap[i][j];
                    if (!cell.Reachable)
                    {
                        cell.setIsObstacle(true);
                    }
                }
            }*/
        }

        private void dfs(Cell[][] bitmap, int column, int row)
        {
            if (column < 0 || column >= bitmap.Length || row < 0 || row >= bitmap[0].Length)
            {
                return;
            }
            Cell cell = bitmap[column][row];
            if (cell.isObstacle() || cell.Reachable)
            {
                return;
            }
            cell.Reachable = true;
            dfs(bitmap, column - 1, row);
            dfs(bitmap, column, row - 1);
            dfs(bitmap, column + 1, row);
            dfs(bitmap, column, row + 1);
        }



        public RectangleF getPitchArea()
        {
            return pitchArea;
        }


        public Cell[][] getObstaclesBitmap()
        {
            return bitmap;
        }


        public int mapRobotXPositionToCellColumnNo(double x)
        {
            return Convert.ToInt32(Math.Round((x - pitchArea.X) / cellWidth));
        }

        public int mapRobotYPositionToCellRowNo(double y)
        {
            return Convert.ToInt32(Math.Round((y - pitchArea.Y) / cellHeight));
        }
    }
}
